#include "separatedConvolution.h"
#include <time.h>

/** Se possibile, separa un kernel in due componenti: una verticale kernelY e una orizzontale kernelX
 *  in modo che sia kernel = kernelY * kernelX
  
  @param kernel: kernel per la convoluzione, deve essere dispari, quadrato e a rango 1
  @param kernelY: componente verticale
  @param kernelX: componente orizzontale
*/
int separateKernel(const Mat_<float> kernel, Mat_<float>& kernelY,Mat_<float>& kernelX){

  //Controllo che il kernel sia quadrato e dispari
  if(kernel.rows != kernel.cols || (kernel.rows%2) == 0 ){
      cerr << "separateKernel(): Dimensioni errate del kernel.\nIl kernel deve essere dispari e quadrato..";
      return -1;
    }


  //Istanzio il decompositore
  SVD decompositore(kernel);

  //Controllo che il kernel sia a rango 1.
  unsigned rango = 0;
  for(int i = 0 ; i < kernel.rows ; ++i){
      if(decompositore.w.at<float>(0,i) > 0.0001f)
        rango++;
    }
  if(rango != 1){
      cerr << "separateKernel(): Kernel non a rango 1, non decomponibile..";
      return -2;
    }

#ifdef DEBUG_ON
  cout << "rango: " << rango << endl;
  cout << "U: "<< endl << decompositore.u<< endl; //U in matlab
  cout << "S: "<< endl << decompositore.w<< endl; //La S in matlab
  cout<<  "S(1,1): "<< endl << decompositore.w.at<float>(0,0) << endl;
  cout << "V: "<< endl << decompositore.vt<< endl; //V in matlab
#endif

  //Prendo le colonne che servono
  decompositore.u.col(0).assignTo(kernelY);
  decompositore.vt.row(0).assignTo(kernelX);
  //Eseguo i calcoli
  Mat_<float>::iterator it,ito;
  for(it = kernelY.begin(),ito = kernelX.begin() ; it != kernelY.end(); ++it, ++ito){
      *it = *it * sqrt(decompositore.w.at<float>(0,0));
      *ito = *ito * sqrt(decompositore.w.at<float>(0,0));
    }

#ifdef DEBUG_ON
  cout <<"kernelY:" << kernelY.rows<<","<< kernelY.cols<<"-->"<< kernelY << endl << endl;
  cout <<"kernelX:" << kernelX.rows<<","<< kernelX.cols<<"-->"<< kernelX << endl << endl;

  //Controllo che il risultato sia corretto
  Mat ris;
  ris = kernelY*kernelX;
  cout << h<< endl<< endl<< endl<< endl;
  cout << ris;
#endif

  return 0;

}



/** Esegue la convoluzione di un kernel su un'immagine utilizzando due kernel, uno per la convoluzione lungo l'asse X 
 * l'altro per la convoluzione lungo l'asse Y
 * @param imgIn immagine su cui si vuole applicare il filtro
 * @param imgOut immagine risultante
 * @param kernelY kernel lungo la componente Y
 * @param kernelX kernel lungo la componente X
 * @return -1 se c'è un errore
 */
int separatedConvolution(const Mat_<uchar>& imgIn , Mat_<uchar>& imgOut,const Mat_<float>& kernelY, const Mat_<float>& kernelX ){

  //Controllo i kernel in ingresso
  if(kernelY.rows != kernelX.cols || kernelX.rows != kernelY.cols || (kernelY.rows%2) == 0){
      cerr << "separatedConvolution(): kernel di dimensioni errate.." << endl;
      return -1;
    }


#ifdef DEBUG_ON
  cout << "kernelY("<<kernelY.rows << "," << kernelY.cols << ")"<< endl;
  cout << "kernelX("<<kernelX.rows << "," << kernelX.cols << ")"<< endl;
#endif

  //Partiamo da una immagine risultante nera
  imgOut = Mat::zeros(imgOut.rows,imgOut.cols,CV_8U);
  Mat_<uchar> imgOut1(imgOut.rows,imgOut.cols);
  imgOut1 = Mat::zeros(imgOut.rows,imgOut.cols,CV_8U);
  //Eseguo la convoluzione con il kernel orrizzontale

  int margine = kernelY.rows/2;

  //Convoluzione in orrizzontale
  for(int r = margine ; r < imgIn.rows-margine ; ++r)
    for(int c = margine ; c < imgIn.cols-margine ; ++c)
      for(int kc=0; kc < kernelY.rows; ++kc)
        imgOut1(r,c) += imgIn(r, c+kc-margine) * kernelY(kc,0);

  //Convoluzione in verticale
  for(int r = margine ; r < imgIn.rows-margine ; ++r)
    for(int c = margine ; c < imgIn.cols-margine ; ++c)
      for(int kr=0; kr<kernelX.cols; ++kr)
        imgOut(r,c) += imgOut1(r+kr-margine, c) * kernelX(0,kr);

  return 0;
}




/** Esegue la convoluzione di un kernel su un'immagine sfruttando la proprietà di separabilità dei kernel separabili
 * @param imgIn immagine su cui si vuole applicare il filtro
 * @param imgOut immagine risultante
 * @param kernelY kernel lungo la componente Y
 * @param kernelX kernel lungo la componente X
 * @return -1 se c'è un errore
 */
int separatedConvolution(const Mat_<uchar>& imgIn , Mat_<uchar>& imgOut,const Mat_<float>& kernel){
  Mat_<float> kery,kerx;
  int r_value;
  r_value = separateKernel(kernel, kery,kerx);
  if(r_value == 0)
    r_value = separatedConvolution(imgIn , imgOut,kery, kerx );
  return r_value;
}



/** Esegue la convoluzione di un kernel su un'immagine utilizzando due kernel, uno per la convoluzione lungo l'asse X
 * l'altro per la convoluzione lungo l'asse Y, utilizzando i puntatori per accedere ai dati
 * @param imgIn immagine su cui si vuole applicare il filtro
 * @param imgOut immagine risultante
 * @param kernelY kernel lungo la componente Y
 * @param kernelX kernel lungo la componente X
 * @param kern_lines numero di righe del kernel
 * @return -1 se c'è un errore
 */
int separatedConvWithPointers(uchar* imgIn , int img_rows, int img_cols, uchar* imgOut,float* kernelY,float* kernelX, int kern_lines){

  //Controllo i kernel in ingresso
  if((kern_lines%2) == 0){
      cerr << "separatedonvWithPointers(): kernel di dimensioni errate.." << endl;
      return -1;
    }

  //Partiamo da una immagine risultante nera
  for(int r=0; r < img_rows ; ++r)
    for(int c=0; c < img_cols; ++c)
      *(imgOut + r*img_cols+c) = 0;
  
  /*
  uchar* end = imgOut+(img_rows*img_cols)
  for(uchar *it = imgOut ; it != end ; ++it){
	*(imgOut) = 0;
  }
  */


  //Carico l'immagine intermedia
  Mat1b outp(img_rows,img_cols);
  outp = Mat::zeros(img_rows,img_cols,CV_8U);
  int margine = kern_lines/2;

  //Eseguo la convoluzione orrizzontale

  //Convoluzione in orrizzontale
  for(int r = margine ; r < img_rows-margine ; ++r)
    for(int c = margine ; c < img_cols-margine ; ++c)
      for(int kc=0; kc < kern_lines; ++kc)
        *(outp.data + (r*img_cols+c)) += *(imgIn + (r*img_cols+c) - margine +kc) * *(kernelX + kc);



#ifdef DEBUG_ON
  imshow("magineIntermedia",outp);
#endif
  //Convoluzione in verticale
  for(int r = margine ; r < img_rows-margine ; ++r)
    for(int c = margine ; c < img_cols-margine ; ++c)
      for(int kr=0; kr < kern_lines; ++kr)
        *(imgOut + (r*img_cols+c)) += *(outp.data + (r*img_cols+c) + (img_cols * (kr-margine))) * *(kernelY + (kr*kern_lines));
  return 0;
}


/** Esegue la convoluzione di un kernel su un'immagine sfruttando la proprietà di separabilità dei kernel separabili
 *  e utilizzando i puntatori per accedere ai dati
 * @param imgIn immagine su cui si vuole applicare il filtro
 * @param imgOut immagine risultante
 * @param kernelY kernel lungo la componente Y
 * @param kernelX kernel lungo la componente X
 * @return -1 se c'è un errore
 */
int separatedConvWithPointers(const Mat_<uchar>& inImg , Mat_<uchar>& outImg,const Mat_<float>& kernel){
  Mat_<float> kery,kerx;
  int r_value;

  r_value = separateKernel(kernel, kery,kerx);

  if(r_value == 0)
    r_value = separatedConvWithPointers(inImg.data , inImg.rows, inImg.cols, outImg.data,(float*) (kery.data),(float*)kerx.data, kery.rows);

  return r_value;
}

/** Funzione per il test della separated convolution */
int testSeparatedConvolution(){
	
	//Immagine di ingresso
	Mat1b inImg = imread("lama.jpg",0);

	//Immagini di uscita
	Mat_<uchar> outImg1(inImg.rows,inImg.cols);
	Mat_<uchar> outImg2(inImg.rows,inImg.cols);

	//Kernel
	//Mat kernel = Mat::ones( 7, 7, CV_32F )/ (float)(7*7); //Kernel di media
	Mat_<float> kernel = getGaussianKernel(23,0.9f) * getGaussianKernel(23,0.9f).t(); //Kernel gaussiano
	
	//Timer
	clock_t startTime;

	namedWindow( "originale", CV_WINDOW_AUTOSIZE );
	imshow( "originale", inImg );
	
	startTime = clock();
	separatedConvWithPointers(inImg, outImg1, kernel);
	cout << "separatedConvWithPointers: "<< double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " secondi." << endl;

	namedWindow( "separated con puntatori", CV_WINDOW_AUTOSIZE );
	imshow( "separated con puntatori", outImg1 );


	startTime = clock();
	separatedConvolution(inImg, outImg2, kernel);
	cout << "separatedConvolution: "<< double( clock() - startTime ) / (double)CLOCKS_PER_SEC<< " secondi." << endl;
	
	namedWindow( "separated", CV_WINDOW_AUTOSIZE );
	imshow( "separated", outImg2 );

	return 0;
}






